#include "wheel_factor.h"

WheelFactor::WheelFactor(std::shared_ptr<WheelPreIntegration> wheel_preintegration) : wheel_preintegration_(wheel_preintegration) {

}

void WheelFactor::ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 3, 1> &residual, Eigen::Matrix<double, 3, 3> &jacobianXi, Eigen::Matrix<double, 3, 3> &jacobianXj) {
    Eigen::Matrix2d Ri = SO2(last_state.R).matrix();
    Eigen::Vector2d ri = last_state.p;
    double ai = SO2(last_state.R).log();
    double aj = SO2(state.R).log();
    Eigen::Vector2d rj = state.p;
 
    // 公式（24）
    residual.head<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;
    residual[2] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();

    // compute jacobian matrix
    Eigen::Vector2d rij = rj-ri;
    Eigen::Vector2d rij_x(-rij[1], rij[0]);
    // 公式（25）
    jacobianXi.block<2,2>(0,0) = -Ri.transpose();
    jacobianXi.block<2,1>(0,2) = -Ri.transpose() * rij_x;
    jacobianXi.block<1,2>(2,0).setZero();
    jacobianXi(2,2) = -1;
 
    jacobianXj.setIdentity();
    jacobianXj.block<2,2>(0,0) = Ri.transpose();
}